#include <Arduino.h>
#include <Adafruit_SSD1306.h>
#include <RevEng_PAJ7620.h>  // PAJ7620U2库文件

// Display Parameters
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// 舵机引脚定义 (使用验证有效的引脚)
#define X_PIN 18  // 舵机X引脚
#define Y_PIN 19  // 舵机Y引脚

// PWM参数
#define PWM_FREQ 50        // 50Hz频率
#define PWM_RESOLUTION 16  // 16位分辨率
#define PWM_CHANNEL_X 0    // X轴PWM通道
#define PWM_CHANNEL_Y 1    // Y轴PWM通道

// 舵机脉宽参数 (微秒) - Arduino标准参数 (已验证有效)
#define SERVO_MIN_PULSE 544   // 0度对应的脉宽
#define SERVO_MAX_PULSE 2400  // 180度对应的脉宽
#define SERVO_CENTER_PULSE 1472 // 90度对应的脉宽

// 舵机控制参数
#define STEP 1
#define SERVO_DELAY 10
#define X_CENTER 90
#define X_OFFSET 20
#define Y_CENTER 90
#define Y_OFFSET 30
int x_min = X_CENTER - X_OFFSET;
int x_max = X_CENTER + X_OFFSET;
int y_min = Y_CENTER - Y_OFFSET;
int y_max = Y_CENTER + Y_OFFSET;

// 当前舵机角度
int current_x_angle = X_CENTER;
int current_y_angle = Y_CENTER;
bool servos_attached = false;

unsigned long last_command_time = 0; // 上次命令执行的时间戳
const unsigned long IDLE_TIMEOUT = 5000; // 空闲超时时间（毫秒）

// Gesture sensor
RevEng_PAJ7620 sensor;  // 创建手势传感器对象
bool gesture_sensor_available = false;  // 手势传感器是否可用

// Eye Parameters
int ref_eye_height = 40;
int ref_eye_width = 40;
int ref_space_between_eye = 10;
int ref_corner_radius = 10;
int blink_count = 0;

// Current Eye State
int left_eye_height = ref_eye_height;
int left_eye_width = ref_eye_width;
int right_eye_x = 32 + ref_eye_width + ref_space_between_eye;
int left_eye_x = 32;
int left_eye_y = 32;
int right_eye_y = 32;
int right_eye_height = ref_eye_height;
int right_eye_width = ref_eye_width;

// 防抖时间戳
unsigned long last_gesture_time = 0;
const unsigned long GESTURE_COOL_DOWN = 10;  // 设置手势冷却时间，避免误识别
uint8_t last_gesture = 255;  // 用于记录上一个手势，初始化为255（无效手势）

// Function Prototypes
void draw_eyes(bool update);
void saccade(int direction_x, int direction_y);
void move_eye(int direction);
void eye_center(bool update);
void eye_blink(int speed);
void eye_sleep();
void eye_wakeup();
void eye_breathe();
void eye_happy();
void eye_sad();
void eye_anger();
void eye_surprise();
void eye_right();
void eye_left();
void head_move(int x_offset, int y_offset, int servo_delay);
void head_up(int offset);
void head_down(int offset);
void head_left(int offset);
void head_right(int offset);
void head_center(int servo_delay);
void head_nod();
void head_shake();
void head_roll(int servo_delay);
void handle_command(String cmd);
void print_help();
void attach_servos();
void detach_servos();
int microsecondsToDutyCycle(int microseconds);
void setServoAngle(int channel, int angle);
int getServoAngle(int channel);

// ===== 原生PWM舵机控制函数 =====

// 将微秒转换为PWM占空比
int microsecondsToDutyCycle(int microseconds) {
  // 限制脉宽范围
  microseconds = constrain(microseconds, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  
  // 计算占空比 (16位分辨率下的值)
  // 周期 = 1/50Hz = 20ms = 20000us
  // 占空比 = (脉宽 / 周期) * (2^16 - 1)
  int dutyCycle = (microseconds * 65535) / 20000;
  
  return dutyCycle;
}

// 设置舵机角度
void setServoAngle(int channel, int angle) {
  // 限制角度范围
  angle = constrain(angle, 0, 180);
  
  // 将角度映射到脉宽
  int pulseWidth = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  
  // 转换为占空比并设置PWM
  int dutyCycle = microsecondsToDutyCycle(pulseWidth);
  ledcWrite(channel, dutyCycle);
  
  // 更新当前角度记录
  if (channel == PWM_CHANNEL_X) {
    current_x_angle = angle;
  } else if (channel == PWM_CHANNEL_Y) {
    current_y_angle = angle;
  }
}

// 获取舵机当前角度
int getServoAngle(int channel) {
  if (channel == PWM_CHANNEL_X) {
    return current_x_angle;
  } else if (channel == PWM_CHANNEL_Y) {
    return current_y_angle;
  }
  return 90; // 默认返回中心角度
}

// Draw Eyes
void draw_eyes(bool update = true) {
  display.clearDisplay();
  // Draw left eye
  int x = int(left_eye_x - left_eye_width / 2);
  int y = int(left_eye_y - left_eye_height / 2);
  display.fillRoundRect(x, y, left_eye_width, left_eye_height, ref_corner_radius, SSD1306_WHITE);
  // Draw right eye
  x = int(right_eye_x - right_eye_width / 2);
  y = int(right_eye_y - right_eye_height / 2);
  display.fillRoundRect(x, y, right_eye_width, right_eye_height, ref_corner_radius, SSD1306_WHITE);
  if (update) display.display();
}

// Center Eyes
void eye_center(bool update = true) {
  left_eye_height = ref_eye_height;
  left_eye_width = ref_eye_width;
  right_eye_height = ref_eye_height;
  right_eye_width = ref_eye_width;
  left_eye_x = SCREEN_WIDTH / 2 - ref_eye_width / 2 - ref_space_between_eye / 2;
  left_eye_y = SCREEN_HEIGHT / 2;
  right_eye_x = SCREEN_WIDTH / 2 + ref_eye_width / 2 + ref_space_between_eye / 2;
  right_eye_y = SCREEN_HEIGHT / 2;
  draw_eyes(update);
}

void eye_happy() {
  eye_center(false);
  //draw inverted triangle over eye lower part
  int offset = ref_eye_height / 2;
  for (int i = 0; i < 10; i++) {
    display.fillTriangle(
      left_eye_x - left_eye_width / 2 - 1, left_eye_y + offset,
      left_eye_x + left_eye_width / 2 + 1, left_eye_y + 5 + offset,
      left_eye_x - left_eye_width / 2 - 1, left_eye_y + left_eye_height + offset,
      SSD1306_BLACK
    );
    display.fillTriangle(
      right_eye_x + right_eye_width / 2 + 1, right_eye_y + offset,
      right_eye_x - left_eye_width / 2 - 1, right_eye_y + 5 + offset,
      right_eye_x + right_eye_width / 2 + 1, right_eye_y + right_eye_height + offset,
      SSD1306_BLACK
    );
    offset -= 2;
    display.display();
  }
  display.display();
  delay(100);
}

void eye_sad() {
  eye_center(false);
  int offset = ref_eye_height / 2;
  for (int i = 0; i < 10; i++) {
    display.fillTriangle(
      left_eye_x - left_eye_width / 2 - 5, left_eye_y - offset + 5,
      left_eye_x + left_eye_width / 2 + 5, left_eye_y - 5 - offset,
      left_eye_x - left_eye_width / 2 - 5, left_eye_y - left_eye_height - offset,
      SSD1306_BLACK
    );
    display.fillTriangle(
      right_eye_x + right_eye_width / 2 + 5, right_eye_y - offset + 5,
      right_eye_x - left_eye_width / 2 - 5, right_eye_y - 5 - offset,
      right_eye_x + right_eye_width / 2 + 5, right_eye_y - left_eye_height - offset,
      SSD1306_BLACK
    );
    offset -= 2;
    display.display();
  }
  display.display();
  delay(100);
}

void eye_anger() {
  eye_center(false);
  int offset = ref_eye_height / 2;
  for (int i = 0; i < 10; i++) {
    display.fillTriangle(
      left_eye_x + left_eye_width / 2 + 5, left_eye_y - offset + 5,
      left_eye_x - left_eye_width / 2 - 5, left_eye_y - 5 - offset,
      left_eye_x + left_eye_width / 2 + 5, left_eye_y - left_eye_height - offset,
      SSD1306_BLACK
    );
    display.fillTriangle(
      right_eye_x - right_eye_width / 2 - 5, right_eye_y - offset + 5,
      right_eye_x + right_eye_width / 2 + 5, right_eye_y - 5 - offset,
      right_eye_x - right_eye_width / 2 - 5, right_eye_y - left_eye_height - offset,
      SSD1306_BLACK
    );
    offset -= 2;
    display.display();
  }
  display.display();
  delay(100);
}

void eye_surprise() {
  eye_center(false);
  int initial_width = left_eye_width;
  int initial_height = left_eye_height;
  int min_width = 10;
  int min_height = 10;
  int corner_radius = ref_corner_radius;

  while (initial_width > min_width && initial_height > min_height) {
    display.clearDisplay();
    int x = int(left_eye_x - initial_width / 2);
    int y = int(left_eye_y - initial_height / 2);
    display.fillRoundRect(x, y, initial_width, initial_height, corner_radius, SSD1306_WHITE);
    x = int(right_eye_x - initial_width / 2);
    y = int(right_eye_y - initial_height / 2);
    display.fillRoundRect(x, y, initial_width, initial_height, corner_radius, SSD1306_WHITE);
    display.display();

    initial_width -= 2;
    initial_height -= 2;
    corner_radius = max(corner_radius - 1, 1);
  }
  delay(100);
}

// 新增搞怪表情函数
void eye_wink() {
  // 眨眼表情 - 一只眼睛闭上，一只眼睛正常
  eye_center(false);
  
  // 左眼正常，右眼闭合
  for (int i = 0; i < 5; i++) {
    display.clearDisplay();
    // 左眼正常
    int x = int(left_eye_x - left_eye_width / 2);
    int y = int(left_eye_y - left_eye_height / 2);
    display.fillRoundRect(x, y, left_eye_width, left_eye_height, ref_corner_radius, SSD1306_WHITE);
    
    // 右眼逐渐闭合
    int right_height = right_eye_height - i * 4;
    if (right_height > 2) {
      x = int(right_eye_x - right_eye_width / 2);
      y = int(right_eye_y - right_height / 2);
      display.fillRoundRect(x, y, right_eye_width, right_height, ref_corner_radius, SSD1306_WHITE);
    }
    display.display();
    delay(50);
  }
  
  delay(300);
  
  // 右眼重新睁开
  for (int i = 4; i >= 0; i--) {
    display.clearDisplay();
    // 左眼正常
    int x = int(left_eye_x - left_eye_width / 2);
    int y = int(left_eye_y - left_eye_height / 2);
    display.fillRoundRect(x, y, left_eye_width, left_eye_height, ref_corner_radius, SSD1306_WHITE);
    
    // 右眼逐渐睁开
    int right_height = right_eye_height - i * 4;
    x = int(right_eye_x - right_eye_width / 2);
    y = int(right_eye_y - right_height / 2);
    display.fillRoundRect(x, y, right_eye_width, right_height, ref_corner_radius, SSD1306_WHITE);
    display.display();
    delay(50);
  }
  delay(100);
}

void eye_crazy() {
  // 疯狂表情 - 眼睛快速变化大小和位置
  eye_center(false);
  
  for (int cycle = 0; cycle < 3; cycle++) {
    for (int i = 0; i < 8; i++) {
      display.clearDisplay();
      
      // 随机变化眼睛大小和位置
      int left_w = left_eye_width + random(-8, 8);
      int left_h = left_eye_height + random(-6, 6);
      int right_w = right_eye_width + random(-8, 8);
      int right_h = right_eye_height + random(-6, 6);
      
      int left_x_offset = random(-6, 6);
      int left_y_offset = random(-4, 4);
      int right_x_offset = random(-6, 6);
      int right_y_offset = random(-4, 4);
      
      // 确保尺寸不会太小
      left_w = max(left_w, 8);
      left_h = max(left_h, 6);
      right_w = max(right_w, 8);
      right_h = max(right_h, 6);
      
      // 绘制左眼
      int x = int(left_eye_x + left_x_offset - left_w / 2);
      int y = int(left_eye_y + left_y_offset - left_h / 2);
      display.fillRoundRect(x, y, left_w, left_h, ref_corner_radius, SSD1306_WHITE);
      
      // 绘制右眼
      x = int(right_eye_x + right_x_offset - right_w / 2);
      y = int(right_eye_y + right_y_offset - right_h / 2);
      display.fillRoundRect(x, y, right_w, right_h, ref_corner_radius, SSD1306_WHITE);
      
      display.display();
      delay(80);
    }
  }
  
  eye_center();
  delay(200);
}

void eye_dizzy() {
  // 眩晕表情 - 眼睛转圈
  eye_center(false);
  
  int radius = 8;
  for (int angle = 0; angle < 720; angle += 30) { // 转两圈
    display.clearDisplay();
    
    // 计算圆周运动的偏移
    float rad = angle * PI / 180.0;
    int x_offset = int(radius * cos(rad));
    int y_offset = int(radius * sin(rad));
    
    // 绘制左眼（顺时针）
    int x = int(left_eye_x + x_offset - left_eye_width / 2);
    int y = int(left_eye_y + y_offset - left_eye_height / 2);
    display.fillRoundRect(x, y, left_eye_width, left_eye_height, ref_corner_radius, SSD1306_WHITE);
    
    // 绘制右眼（逆时针）
    x = int(right_eye_x - x_offset - right_eye_width / 2);
    y = int(right_eye_y - y_offset - right_eye_height / 2);
    display.fillRoundRect(x, y, right_eye_width, right_eye_height, ref_corner_radius, SSD1306_WHITE);
    
    display.display();
    delay(60);
  }
  
  eye_center();
  delay(200);
}

void eye_sleepy() {
  // 困倦表情 - 眼睛慢慢闭合又睁开
  eye_center(false);
  
  // 慢慢闭眼
  for (int i = 0; i < 10; i++) {
    display.clearDisplay();
    
    int height = left_eye_height - i * 2;
    if (height < 2) height = 2;
    
    // 绘制左眼
    int x = int(left_eye_x - left_eye_width / 2);
    int y = int(left_eye_y - height / 2);
    display.fillRoundRect(x, y, left_eye_width, height, ref_corner_radius, SSD1306_WHITE);
    
    // 绘制右眼
    x = int(right_eye_x - right_eye_width / 2);
    y = int(right_eye_y - height / 2);
    display.fillRoundRect(x, y, right_eye_width, height, ref_corner_radius, SSD1306_WHITE);
    
    display.display();
    delay(100);
  }
  
  delay(500); // 闭眼停留
  
  // 慢慢睁眼
  for (int i = 9; i >= 0; i--) {
    display.clearDisplay();
    
    int height = left_eye_height - i * 2;
    if (height < 2) height = 2;
    
    // 绘制左眼
    int x = int(left_eye_x - left_eye_width / 2);
    int y = int(left_eye_y - height / 2);
    display.fillRoundRect(x, y, left_eye_width, height, ref_corner_radius, SSD1306_WHITE);
    
    // 绘制右眼
    x = int(right_eye_x - right_eye_width / 2);
    y = int(right_eye_y - height / 2);
    display.fillRoundRect(x, y, right_eye_width, height, ref_corner_radius, SSD1306_WHITE);
    
    display.display();
    delay(120);
  }
  
  delay(200);
}

void draw_heart(int center_x, int center_y, int size);

void eye_confused() {
  // 困惑表情 - 一大一小的眼睛
  eye_center(false);
  
  for (int i = 0; i < 3; i++) {
    display.clearDisplay();
    
    // 左眼变大
    int left_w = left_eye_width + 6;
    int left_h = left_eye_height + 4;
    int x = int(left_eye_x - left_w / 2);
    int y = int(left_eye_y - left_h / 2);
    display.fillRoundRect(x, y, left_w, left_h, ref_corner_radius, SSD1306_WHITE);
    
    // 右眼变小
    int right_w = right_eye_width - 4;
    int right_h = right_eye_height - 3;
    x = int(right_eye_x - right_w / 2);
    y = int(right_eye_y - right_h / 2);
    display.fillRoundRect(x, y, right_w, right_h, ref_corner_radius, SSD1306_WHITE);
    
    display.display();
    delay(400);
    
    display.clearDisplay();
    
    // 左眼变小
    left_w = left_eye_width - 4;
    left_h = left_eye_height - 3;
    x = int(left_eye_x - left_w / 2);
    y = int(left_eye_y - left_h / 2);
    display.fillRoundRect(x, y, left_w, left_h, ref_corner_radius, SSD1306_WHITE);
    
    // 右眼变大
    right_w = right_eye_width + 6;
    right_h = right_eye_height + 4;
    x = int(right_eye_x - right_w / 2);
    y = int(right_eye_y - right_h / 2);
    display.fillRoundRect(x, y, right_w, right_h, ref_corner_radius, SSD1306_WHITE);
    
    display.display();
    delay(400);
  }
  
  eye_center();
  delay(200);
}

void eye_love() {
  // 爱心眼睛表情 - 两只眼睛都变成爱心
  eye_center(false);
  
  for (int frame = 0; frame < 4; frame++) {
    display.clearDisplay();
    
    // 绘制左眼爱心（增大尺寸）
    draw_heart(left_eye_x, left_eye_y, 18);
    
    // 绘制右眼爱心（增大尺寸）
    draw_heart(right_eye_x, right_eye_y, 18);
    
    display.display();
    delay(1000);  // 延长显示时间
    
    // 爱心闪烁效果
    if (frame < 3) {
      display.clearDisplay();
      draw_heart(left_eye_x, left_eye_y, 15);
      draw_heart(right_eye_x, right_eye_y, 15);
      display.display();
      delay(400);  // 延长闪烁时间
    }
  }
  
  delay(800);  // 延长结束延迟
  eye_center();
}

void draw_heart(int center_x, int center_y, int size) {
  // 绘制爱心形状
  // 爱心由两个圆和一个三角形组成
  
  int radius = size / 3;
  int heart_width = size;
  int heart_height = size;
  
  // 绘制左上圆
  int left_circle_x = center_x - radius / 2;
  int left_circle_y = center_y - radius / 2;
  display.fillCircle(left_circle_x, left_circle_y, radius, SSD1306_WHITE);
  
  // 绘制右上圆
  int right_circle_x = center_x + radius / 2;
  int right_circle_y = center_y - radius / 2;
  display.fillCircle(right_circle_x, right_circle_y, radius, SSD1306_WHITE);
  
  // 绘制下方三角形（爱心的尖端）
  int triangle_top_y = center_y;
  int triangle_bottom_x = center_x;
  int triangle_bottom_y = center_y + heart_height / 2;
  int triangle_left_x = center_x - heart_width / 2;
  int triangle_right_x = center_x + heart_width / 2;
  
  display.fillTriangle(
    triangle_left_x, triangle_top_y,
    triangle_right_x, triangle_top_y,
    triangle_bottom_x, triangle_bottom_y,
    SSD1306_WHITE
  );
  
  // 填充中间的矩形区域，让爱心更饱满
  display.fillRect(
    center_x - heart_width / 2, 
    center_y - radius,
    heart_width, 
    radius + 2,
    SSD1306_WHITE
  );
}

void saccade(int direction_x, int direction_y) {
  //quick movement of the eye, no size change. stay at position after movement, will not move back,  call again with opposite direction
  //direction == -1 :  move left
  //direction == 1 :  move right

  int direction_x_movement_amplitude = 8;
  int direction_y_movement_amplitude = 6;
  int eye_blink_amplitude = 8;

  for (int i = 0; i < 1; i++) {
    left_eye_x += direction_x_movement_amplitude * direction_x;
    right_eye_x += direction_x_movement_amplitude * direction_x;
    left_eye_y += direction_y_movement_amplitude * direction_y;
    right_eye_y += direction_y_movement_amplitude * direction_y;
    right_eye_height -= eye_blink_amplitude;
    left_eye_height -= eye_blink_amplitude;
    draw_eyes();
    delay(1);
  }

  for (int i = 0; i < 1; i++) {
    left_eye_x += direction_x_movement_amplitude * direction_x;
    right_eye_x += direction_x_movement_amplitude * direction_x;
    left_eye_y += direction_y_movement_amplitude * direction_y;
    right_eye_y += direction_y_movement_amplitude * direction_y;
    right_eye_height += eye_blink_amplitude;
    left_eye_height += eye_blink_amplitude;
    draw_eyes();
    delay(1);
  }
}

void move_eye(int direction) {  // MOVES TO RIGHT OR LEFT DEPENDING ON 1 OR -1 INPUT.
  //direction == -1 :  move left
  //direction == 1 :  move right

  int direction_oversize = 1;
  int direction_movement_amplitude = 2;
  int eye_blink_amplitude = 5;

  for (int i = 0; i < 3; i++) {
    left_eye_x += direction_movement_amplitude * direction;
    right_eye_x += direction_movement_amplitude * direction;
    right_eye_height -= eye_blink_amplitude;
    left_eye_height -= eye_blink_amplitude;
    if (direction > 0) {
      right_eye_height += direction_oversize;
      right_eye_width += direction_oversize;
    } else {
      left_eye_height += direction_oversize;
      left_eye_width += direction_oversize;
    }

    draw_eyes();
    delay(1);
  }
  for (int i = 0; i < 3; i++) {
    left_eye_x += direction_movement_amplitude * direction;
    right_eye_x += direction_movement_amplitude * direction;
    right_eye_height += eye_blink_amplitude;
    left_eye_height += eye_blink_amplitude;
    if (direction > 0) {
      right_eye_height += direction_oversize;
      right_eye_width += direction_oversize;
    } else {
      left_eye_height += direction_oversize;
      left_eye_width += direction_oversize;
    }
    draw_eyes();
    delay(1);
  }

  delay(1000);

  for (int i = 0; i < 3; i++) {
    left_eye_x -= direction_movement_amplitude * direction;
    right_eye_x -= direction_movement_amplitude * direction;
    right_eye_height -= eye_blink_amplitude;
    left_eye_height -= eye_blink_amplitude;
    if (direction > 0) {
      right_eye_height -= direction_oversize;
      right_eye_width -= direction_oversize;
    } else {
      left_eye_height -= direction_oversize;
      left_eye_width -= direction_oversize;
    }
    draw_eyes();
    delay(1);
  }
  for (int i = 0; i < 3; i++) {
    left_eye_x -= direction_movement_amplitude * direction;
    right_eye_x -= direction_movement_amplitude * direction;
    right_eye_height += eye_blink_amplitude;
    left_eye_height += eye_blink_amplitude;
    if (direction > 0) {
      right_eye_height -= direction_oversize;
      right_eye_width -= direction_oversize;
    } else {
      left_eye_height -= direction_oversize;
      left_eye_width -= direction_oversize;
    }
    draw_eyes();
    delay(1);
  }
  eye_center();
}

void eye_right() {
  move_eye(1);
}

void eye_left() {
  move_eye(-1);
}

void head_right(int offset=10) {
  head_move(offset, 0, SERVO_DELAY);
}

void head_left(int offset=10) {
  head_move(-offset, 0, SERVO_DELAY);
}

void head_down(int offset=10) {
  head_move(0, offset, SERVO_DELAY);
}

void head_up(int offset=10) {
  head_move(0, -offset, SERVO_DELAY);
}

void head_roll(int servo_delay=SERVO_DELAY) {
  head_center(SERVO_DELAY);
  head_down(Y_OFFSET/2+5);
  head_move(X_OFFSET, -Y_OFFSET/2, servo_delay);
  head_move(-X_OFFSET, -Y_OFFSET/2, servo_delay);
  head_move(-X_OFFSET, Y_OFFSET/2, servo_delay);
  head_move(X_OFFSET, Y_OFFSET/2, servo_delay);
  head_move(-X_OFFSET, -Y_OFFSET/2, servo_delay);
  head_move(X_OFFSET, -Y_OFFSET/2, servo_delay);
  head_move(X_OFFSET, Y_OFFSET/2, servo_delay);
  head_move(-X_OFFSET, Y_OFFSET/2, servo_delay);
  head_center(SERVO_DELAY);
}

// 产生随机的眼神表情和瞥视效果
void random_eye_expression_with_glance() {
  static unsigned long last_expression_time = 0;
  unsigned long current_time = millis();
  // 控制表情变化的频率，避免频繁变化
  if (current_time - last_expression_time < 5000) { // 5秒内不改变表情
    return;
  }
  
  // 更新表情时间戳
  last_expression_time = current_time;
  
  int random_expr = random(0, 16); // 增加表情种类到16种
  int glance_offset = random(-5, 5); // 随机偏移量，模拟瞥视

  Serial.print("Random expression: ");
  Serial.println(random_expr);

  // 随机选择一个表情
  switch (random_expr) {
    case 0: // 困倦/疲劳
      Serial.println("Expression: Sad");
      eye_sad();
      break;
    case 1: // 疑惑/好奇 - 眼睛偏移
      Serial.println("Expression: Curious glance");
      left_eye_x = SCREEN_WIDTH / 2 - ref_eye_width / 2 - ref_space_between_eye / 2 + glance_offset;
      right_eye_x = SCREEN_WIDTH / 2 + ref_eye_width / 2 + ref_space_between_eye / 2 + glance_offset;
      draw_eyes();
      break;
    case 2: // 愤怒/不满
      Serial.println("Expression: Anger");
      eye_anger();
      break;
    case 3: // 开心/愉悦
      Serial.println("Expression: Happy");
      eye_happy();
      break;
    case 4: // 惊讶
      Serial.println("Expression: Surprise");
      eye_surprise();
      break;
    case 5: // 瞥视效果
      Serial.println("Expression: Glance left-right");
      head_left(10);
      eye_left();
      delay(800);
      head_center(SERVO_DELAY);
      delay(500);
      head_right(10);
      eye_right();
      delay(800);
      head_center(SERVO_DELAY);
      eye_center();
      break;
    case 6: // 眨眼表情
      Serial.println("Expression: Blink");
      eye_blink(3);
      break;
    case 7: // 正常状态
       Serial.println("Expression: Center");
       eye_center();
       break;
     case 8: // 好奇表情 - 小幅度头部移动配合眼神
       Serial.println("Expression: Curious glance");
       {
         // 小幅度随机头部移动
         int curious_x = X_CENTER + random(-10, 11);  // 中心±10度
         int curious_y = Y_CENTER + random(-8, 9);    // 中心±8度
         
         // 确保在安全范围内
         curious_x = constrain(curious_x, x_min, x_max);
         curious_y = constrain(curious_y, y_min, y_max);
         
         setServoAngle(PWM_CHANNEL_X, curious_x);
         setServoAngle(PWM_CHANNEL_Y, curious_y);
         
         // 好奇的眼神序列
         eye_surprise();  // 先惊讶
         delay(800);
         eye_left();      // 左看
         delay(600);
         eye_right();     // 右看
         delay(600);
         eye_center();    // 回中心
         
         // 回到中心位置
         delay(300);
         head_center(SERVO_DELAY);
       }
       break;
     case 9: // 眨眼表情（搞怪）
       Serial.println("Expression: Wink");
       eye_wink();
       break;
     case 10: // 疯狂表情（搞怪）
       Serial.println("Expression: Crazy");
       eye_crazy();
       break;
     case 11: // 眩晕表情（搞怪）
       Serial.println("Expression: Dizzy");
       eye_dizzy();
       break;
     case 12: // 困倦表情（搞怪）
       Serial.println("Expression: Sleepy");
       eye_sleepy();
       break;
      case 13: // 困惑表情（搞怪）
      Serial.println("Expression: Confused");
      eye_confused();
      break;
      case 14: // 爱心表情（搞怪）
      Serial.println("Expression: Love");
      eye_love();
      break;
    case 15: // 眼神左右移动
      Serial.println("Expression: Saccade movement");
      saccade(-1, 0);
      delay(200);
      saccade(1, 0);
      delay(200);
      saccade(0, 0);
      break;
    default: // 默认：正常状态
      Serial.println("Expression: Default center");
      eye_center();
      break;
  }
}

// Handle Gesture
void handle_gesture(uint8_t gesture) {
  // 防止连续重复执行相同的手势动作
  if (millis() - last_gesture_time > GESTURE_COOL_DOWN) {  // 防抖，间隔500ms触发一次
    if (gesture != 255 && gesture != last_gesture) {  // 如果是有效的手势且与上次手势不同
      Serial.print("Gesture detected: ");
      Serial.println(gesture);
      
      switch (gesture) {
        case 0:  // 如果是手势“挥手”
          head_nod();
          break;
        case 1:  // 如果是手势“向上”
          head_up(10);
          break;
        case 2:  // 如果是手势“向下”
          head_down(10);
          break;
        case 3:  // 如果是手势“向左”
          head_left(10);
          break;
        case 4:  // 如果是手势“向右”
          head_right(10);
          break;
        default:
          Serial.println("Unknown gesture");
          break;
      }
      last_gesture = gesture;  // 更新上一个手势
    }
    last_gesture_time = millis();  // 更新手势检测时间
  }
}


// Blink Eyes
// 调整眨眼函数
void eye_blink(int speed = 10) {
  blink_count++;
  Serial.print("Blink count: ");
  Serial.println(blink_count);

  // 让眼睛先闭合
  for (int i = left_eye_height; i > 2; i -= speed) {
    left_eye_height = i;
    right_eye_height = i;
    
    // 保持水平位置一致，避免两个眼睛重合
    left_eye_x = SCREEN_WIDTH / 2 - ref_eye_width / 2 - ref_space_between_eye / 2;
    right_eye_x = SCREEN_WIDTH / 2 + ref_eye_width / 2 + ref_space_between_eye / 2;

    draw_eyes();
    delay(30);
  }

  // 然后让眼睛慢慢恢复
  for (int i = 2; i <= ref_eye_height; i += speed) {
    left_eye_height = i;
    right_eye_height = i;

    left_eye_x = SCREEN_WIDTH / 2 - ref_eye_width / 2 - ref_space_between_eye / 2;
    right_eye_x = SCREEN_WIDTH / 2 + ref_eye_width / 2 + ref_space_between_eye / 2;

    draw_eyes();
    delay(30);
  }
}



void eye_move(int x_offset, int y_offset, int speed = 10) {
  // 调整左眼和右眼的位置
  left_eye_x = constrain(left_eye_x + x_offset, 0, SCREEN_WIDTH - left_eye_width);
  left_eye_y = constrain(left_eye_y + y_offset, 0, SCREEN_HEIGHT - left_eye_height);
  right_eye_x = constrain(right_eye_x + x_offset, 0, SCREEN_WIDTH - right_eye_width);
  right_eye_y = constrain(right_eye_y + y_offset, 0, SCREEN_HEIGHT - right_eye_height);

  draw_eyes();
  delay(speed);
}

// 轻微调整眼睛位置并平滑过渡
void eye_move_smooth(int x_offset, int y_offset, int steps = 10, int speed = 10) {
  int x_step = x_offset / steps;
  int y_step = y_offset / steps;

  for (int i = 0; i < steps; i++) {
    left_eye_x = constrain(left_eye_x + x_step, 0, SCREEN_WIDTH - left_eye_width);
    left_eye_y = constrain(left_eye_y + y_step, 0, SCREEN_HEIGHT - left_eye_height);
    right_eye_x = constrain(right_eye_x + x_step, 0, SCREEN_WIDTH - right_eye_width);
    right_eye_y = constrain(right_eye_y + y_step, 0, SCREEN_HEIGHT - right_eye_height);

    // 轻微调整瞳孔位置，模拟眼球转动
    if (x_step != 0) {
      left_eye_x += (x_step > 0) ? 1 : -1;
      right_eye_x += (x_step > 0) ? 1 : -1;
    }
    if (y_step != 0) {
      left_eye_y += (y_step > 0) ? 1 : -1;
      right_eye_y += (y_step > 0) ? 1 : -1;
    }

    draw_eyes(false); // 更新位置但不改变瞳孔大小
    delay(speed);
  }
}

void eye_look_left_smooth() {
  eye_move_smooth(-15, 0); // 平滑地向左转动
}

void eye_look_right_smooth() {
  eye_move_smooth(15, 0); // 平滑地向右转动
}

void eye_look_up_smooth() {
  eye_move_smooth(0, -15); // 平滑地向上转动
}

void eye_look_down_smooth() {
  eye_move_smooth(0, 15); // 平滑地向下转动
}

// Eye Sleep
void eye_sleep() {
  left_eye_height = 2;
  right_eye_height = 2;
  draw_eyes();
}

// Eye Wakeup
void eye_wakeup() {
  for (int h = 2; h <= ref_eye_height; h += 2) {
    left_eye_height = h;
    right_eye_height = h;
    draw_eyes();
    delay(50);
  }
}

// Head Nod
void head_nod() {
  attach_servos(); // 确保伺服已连接
  int initial_y = getServoAngle(PWM_CHANNEL_Y); // 当前 Y 轴角度
  int down_angle = constrain(initial_y + 15, y_min, y_max); // 向下限制
  int up_angle = constrain(initial_y - 15, y_min, y_max);   // 向上限制

  for (int i = 0; i < 3; i++) {
    setServoAngle(PWM_CHANNEL_Y, down_angle); // 向下
    delay(200);
    setServoAngle(PWM_CHANNEL_Y, up_angle);   // 向上
    delay(200);
  }
  setServoAngle(PWM_CHANNEL_Y, initial_y);   // 返回初始位置
}

void head_shake() {
  attach_servos(); // 确保伺服已连接
  int initial_x = getServoAngle(PWM_CHANNEL_X); // 当前 X 轴角度
  int right_angle = constrain(initial_x + 15, x_min, x_max); // 向右限制
  int left_angle = constrain(initial_x - 15, x_min, x_max);  // 向左限制

  for (int i = 0; i < 3; i++) {
    setServoAngle(PWM_CHANNEL_X, right_angle); // 向右
    delay(200);
    setServoAngle(PWM_CHANNEL_X, left_angle);  // 向左
    delay(200);
  }
  setServoAngle(PWM_CHANNEL_X, initial_x);    // 返回初始位置
}

void eye_breathe() {
  static bool shrinking = true; // 记录当前是收缩还是扩展
  static int min_height = ref_eye_height / 2;
  static int max_height = ref_eye_height;
  
  if (shrinking) {
    left_eye_height -= 1;
    right_eye_height -= 1;
    if (left_eye_height <= min_height) shrinking = false;
  } else {
    left_eye_height += 1;
    right_eye_height += 1;
    if (left_eye_height >= max_height) shrinking = true;
  }
  draw_eyes();
  delay(50); // 控制呼吸节奏
}

// Head Movement
void head_move(int x_offset, int y_offset, int servo_delay) {
  attach_servos(); // 确保伺服被附加
  last_command_time = millis(); // 更新最后一次命令时间
  int x_angle = getServoAngle(PWM_CHANNEL_X);
  int y_angle = getServoAngle(PWM_CHANNEL_Y);
  
  Serial.print("Head move - Current X: ");
  Serial.print(x_angle);
  Serial.print(", Y: ");
  Serial.print(y_angle);
  Serial.print(", Offset X: ");
  Serial.print(x_offset);
  Serial.print(", Y: ");
  Serial.println(y_offset);
  
  int to_x_angle = constrain(x_angle + x_offset, x_min, x_max);
  int to_y_angle = constrain(y_angle + y_offset, y_min, y_max);
  
  Serial.print("Target X: ");
  Serial.print(to_x_angle);
  Serial.print(", Y: ");
  Serial.print(to_y_angle);
  Serial.print(" (Range X: ");
  Serial.print(x_min);
  Serial.print("-");
  Serial.print(x_max);
  Serial.print(", Y: ");
  Serial.print(y_min);
  Serial.print("-");
  Serial.print(y_max);
  Serial.println(")");

  while (x_angle != to_x_angle || y_angle != to_y_angle) {
    if (x_angle != to_x_angle) {
      x_angle += (to_x_angle > x_angle ? STEP : -STEP);
      setServoAngle(PWM_CHANNEL_X, x_angle);
      Serial.print("Moving X to: ");
      Serial.println(x_angle);
    }
    if (y_angle != to_y_angle) {
      y_angle += (to_y_angle > y_angle ? STEP : -STEP);
      setServoAngle(PWM_CHANNEL_Y, y_angle);
      Serial.print("Moving Y to: ");
      Serial.println(y_angle);
    }
    delay(servo_delay);
  }
  Serial.println("Head move completed");
}

// Center Head
void head_center(int servo_delay) {
  head_move(X_CENTER - getServoAngle(PWM_CHANNEL_X), Y_CENTER - getServoAngle(PWM_CHANNEL_Y), servo_delay);
}

// Detach Servos (原生PWM版本)
void detach_servos() {
  if (servos_attached) {
    // 停止PWM输出
    ledcWrite(PWM_CHANNEL_X, 0);
    ledcWrite(PWM_CHANNEL_Y, 0);
    
    // 分离PWM通道
    ledcDetachPin(X_PIN);
    ledcDetachPin(Y_PIN);
    
    servos_attached = false;
    Serial.println("Servos detached (PWM stopped).");
  }
}

// Attach Servos (原生PWM版本)
void attach_servos() {
  if (!servos_attached) {
    // 配置PWM通道
    ledcSetup(PWM_CHANNEL_X, PWM_FREQ, PWM_RESOLUTION);
    ledcSetup(PWM_CHANNEL_Y, PWM_FREQ, PWM_RESOLUTION);
    
    // 将PWM通道绑定到GPIO引脚
    ledcAttachPin(X_PIN, PWM_CHANNEL_X);
    ledcAttachPin(Y_PIN, PWM_CHANNEL_Y);
    
    // 设置为中心位置
    setServoAngle(PWM_CHANNEL_X, X_CENTER);
    setServoAngle(PWM_CHANNEL_Y, Y_CENTER);
    
    servos_attached = true;
    
    Serial.print("Servo X attached to pin ");
    Serial.print(X_PIN);
    Serial.print(" at angle: ");
    Serial.println(X_CENTER);
    
    Serial.print("Servo Y attached to pin ");
    Serial.print(Y_PIN);
    Serial.print(" at angle: ");
    Serial.println(Y_CENTER);
    
    delay(500); // 给舵机时间移动到位置
  }
}

// Handle Serial Commands
void handle_command(String cmd) {
  cmd.trim();
  if (!servos_attached) {
    attach_servos(); // 确保伺服被附加
  }
  last_command_time = millis(); // 更新最后一次命令时间
  Serial.print("Received command: ");
  Serial.println(cmd);

  if (cmd == "blink") {
    eye_blink(10);
  } else if (cmd == "sleep") {
    eye_sleep();
  } else if (cmd == "wakeup") {
    eye_wakeup();
  } else if (cmd == "draw") {
    draw_eyes();
  } else if (cmd == "breathe") {
    eye_breathe();
  } else if (cmd == "head_center") {
    head_center(10);
  } else if (cmd == "nod") {
    head_nod();
  } else if (cmd == "happy") {  
    eye_happy();
  } else if (cmd == "sad") {  
    eye_sad();
  } else if (cmd == "anger") {  
    eye_anger();
  } else if (cmd == "surprise") {  
    eye_surprise();
  } else if (cmd == "right") {  
    eye_right();
  } else if (cmd == "left") {  
    eye_left();
  } else if (cmd == "roll") {
    head_roll(30);
  } else if (cmd == "shake") {
    Serial.println("Executing head_shake..."); // 调试输出
    head_shake();
  } else if (cmd == "help") {
    print_help();
  } else {
    Serial.print("Unknown command: ");
    Serial.println(cmd);
  }
}


// Print Help
void print_help() {
  Serial.println("Available commands:");
  Serial.println("  blink      - Blink the eyes");
  Serial.println("  sleep      - Put eyes to sleep");
  Serial.println("  wakeup     - Wake up the eyes");
  Serial.println("  breathe    - Eye breathing effect");
  Serial.println("  head_center - Center the head");
  Serial.println("  nod        - Nod the head");
  Serial.println("  shake      - Shake the head");
  Serial.println("  help       - Show this help message");
}

// Setup
void setup() {
  Serial.begin(115200);
  Serial.println("Initializing...");
  attach_servos(); // 初始时附加伺服
  
  // 读取并显示当前舵机角度
  delay(100); // 确保舵机已经稳定
  int current_x = getServoAngle(PWM_CHANNEL_X);
  int current_y = getServoAngle(PWM_CHANNEL_Y);
  Serial.println("=== Current Servo Positions ===");
  Serial.print("Servo X (Pin ");
  Serial.print(X_PIN);
  Serial.print(") current angle: ");
  Serial.println(current_x);
  Serial.print("Servo Y (Pin ");
  Serial.print(Y_PIN);
  Serial.print(") current angle: ");
  Serial.println(current_y);
  Serial.println("================================");
  
  // 初始化传感器
  if (sensor.begin()) {
    gesture_sensor_available = true;
    Serial.println("PAJ7620 gesture sensor initialized successfully");
  } else {
    gesture_sensor_available = false;
    Serial.println("PAJ7620 gesture sensor not found - using random actions instead");
  }
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("SSD1306 allocation failed!");
    while (true);
  }
  display.clearDisplay();
  eye_sleep();
  head_down(100);
  delay(1000);
  eye_wakeup();
  eye_center();
  eye_blink();
  head_center(SERVO_DELAY);
  delay(500);
  Serial.println("Initialization complete. Type 'help' for commands.");
}

// 表情更新的时间间隔
const unsigned long EXPRESSION_UPDATE_INTERVAL = 8000; // 8秒更新一次表情
unsigned long last_expression_time = 0;  // 上次更新表情的时间
// Loop
void loop() {
  // 读取手势数据（仅当传感器可用时）
  if (gesture_sensor_available) {
    uint8_t gesture = sensor.readGesture();  // 获取手势数据
    handle_gesture(gesture);  // 处理手势并执行相应动作
  } else {
    // 如果没有手势传感器，使用随机动作
    static unsigned long last_random_action = 0;
    if (millis() - last_random_action > 6000) {  // 每6秒执行一次随机动作
      last_random_action = millis();
      int random_action = random(0, 20);  // 扩展到20种动作
      Serial.print("Random action: ");
      Serial.println(random_action);
      
      // 将16和17也映射到好奇行为，保持其较高出现概率
      if (random_action == 16 || random_action == 17) {
        random_action = 10;  // 映射到好奇行为
      }
      
      switch (random_action) {
        case 0:
          Serial.println("Action: Head nod");
          head_nod();
          break;
        case 1:
          Serial.println("Action: Head shake");
          head_shake();
          break;
        case 2:
          Serial.println("Action: Happy expression");
          eye_happy();
          delay(2000);
          eye_center();
          break;
        case 3:
          Serial.println("Action: Surprise expression");
          eye_surprise();
          delay(2000);
          eye_center();
          break;
        case 4:
          Serial.println("Action: Head roll");
          head_roll(30);
          break;
        case 5:
          Serial.println("Action: Sad expression");
          eye_sad();
          delay(2000);
          eye_center();
          break;
        case 6:
          Serial.println("Action: Anger expression");
          eye_anger();
          delay(2000);
          eye_center();
          break;
        case 7:
          Serial.println("Action: Blink sequence");
          eye_blink(5);
          break;
        case 8:
          Serial.println("Action: Look around");
          eye_left();
          delay(1000);
          eye_right();
          delay(1000);
          eye_center();
          break;
        case 9:
          Serial.println("Action: Head tilt and expression");
          head_left(15);
          eye_happy();
          delay(1500);
          head_center(SERVO_DELAY);
          eye_center();
          break;
        case 10:
          Serial.println("Action: Curious behavior");
          // 好奇行为：大幅度转动身体 + 抬头低头 + 眼睛四处张望
          {
            // 随机生成角度（在安全范围内）
            int random_x = random(x_min, x_max + 1);  // 随机水平角度
            int random_y = random(y_min, y_max + 1);  // 随机垂直角度
            
            Serial.print("Curious: Moving to X=");
            Serial.print(random_x);
            Serial.print(", Y=");
            Serial.println(random_y);
            
            // 大幅度转动身体到随机位置
            setServoAngle(PWM_CHANNEL_X, random_x);
            setServoAngle(PWM_CHANNEL_Y, random_y);
            delay(500);
            
            // 眼睛四处张望序列
            eye_left();
            delay(800);
            eye_right();
            delay(800);
            eye_center();
            delay(300);
            
            // 再次随机移动头部
            int random_x2 = random(x_min, x_max + 1);
            int random_y2 = random(y_min, y_max + 1);
            setServoAngle(PWM_CHANNEL_X, random_x2);
            setServoAngle(PWM_CHANNEL_Y, random_y2);
            delay(500);
            
            // 更多眼睛张望
            eye_surprise();  // 好奇的惊讶表情
            delay(1000);
            eye_left();
            delay(600);
            eye_right();
            delay(600);
            
            // 回到中心位置
            head_center(SERVO_DELAY);
            eye_center();
            delay(500);
          }
          break;
        case 11:
          Serial.println("Action: Wink expression");
          eye_wink();
          delay(1000);
          break;
        case 12:
           Serial.println("Action: Crazy expression");
           eye_crazy();
           delay(1000);
           break;
         case 13:
           Serial.println("Action: Dizzy expression");
           eye_dizzy();
           delay(1000);
           break;
         case 14:
           Serial.println("Action: Sleepy expression");
           eye_sleepy();
           delay(1000);
           break;
         case 15:
          Serial.println("Action: Confused expression");
          eye_confused();
          delay(1000);
          break;
        case 16:
          Serial.println("Action: Love expression");
          eye_love();
          delay(1000);
          break;
        case 17:
          Serial.println("Action: Sleepy expression");
          eye_sleepy();
          delay(1000);
          break;
        case 18:
        case 19:
          Serial.println("Action: Enhanced curiosity behavior");
          {
            int random_x = random(x_min, x_max + 1);
            int random_y = random(y_min, y_max + 1);
            setServoAngle(PWM_CHANNEL_X, random_x);
            setServoAngle(PWM_CHANNEL_Y, random_y);
            delay(500);
            eye_surprise();
            delay(800);
            eye_left();
            delay(600);
            eye_right();
            delay(600);
            head_center(SERVO_DELAY);
            eye_center();
          }
          break;
        }
    }
  }

  // 检查是否有串口命令
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    handle_command(cmd);
    return; // 执行命令后退出
  }
  // 隔一定时间更新表情
  if (millis() - last_expression_time > EXPRESSION_UPDATE_INTERVAL) {
    last_expression_time = millis();  // 更新表情时间
    random_eye_expression_with_glance();  // 随机变化表情
  }
  if (millis() - last_command_time > IDLE_TIMEOUT) {
    // 眨眼
    eye_blink(10);
     // 眼神转动
     eye_look_left_smooth();
     delay(500);
     eye_look_right_smooth();
     delay(500);
     eye_look_up_smooth();
     delay(500);
     eye_look_down_smooth();
     delay(500);
  }
  // 检查是否超过空闲时间，超过则分离伺服电机
  if (millis() - last_command_time > IDLE_TIMEOUT && servos_attached) {
    detach_servos();
  }

  delay(10);
}
